#include "pid.h"
#include <math.h>
void pid_init(PIDController *c,
              float kp, float ki, float kd, float dt,
              float imax, float imin,
              float omax, float omin)
{
    c->kp = kp;
    c->ki = ki;
    c->kd = kd;
    c->dt = dt;

    c->integral_max = imax;
    c->integral_min = imin;
    c->output_max = omax;
    c->output_min = omin;

    c->previous_error = 0;
    c->integral = 0;
    c->target = 0;
    c->setpoint = 0;     // 当前目标 = 0
    c->output = 0;

    // 默认启用斜坡，步长为 0（表示不启用）
    c->ramp_step = 0.0f;
    c->ramp_enable = 1;
	
	c->integral_damping_ratio = 0.3f;
	c->previous_measurement = 0;  // 第一次调用时传入
	c->error = 0;
}
// 启用/禁用斜坡
void pid_enable_ramp(PIDController *c, uint8_t enable) {
    c->ramp_enable = enable;
}

// 设置斜坡步长（单位：目标单位/次 update）
void pid_set_ramp_step(PIDController *c, float step) {
    c->ramp_step = step;
}
void pid_set_target(PIDController *c, float target) {
    c->target = target;  // 记录最终目标
	
	c->integral = 0;//清除积分
	
    // 如果禁用斜坡 或 步长为 0，直接跳转
    if (!c->ramp_enable || c->ramp_step == 0.0f) {
        c->setpoint = target;
    }
    // 否则由 update 处理斜坡
}

// ***********************
// 🔹 分段目标斜坡（Two-Stage Ramp）
// 目标：快速响应 + 平稳到达
// 策略：
//   dist > 30: 全速逼近（base_step）
//   20 < dist ≤ 30: 减速至 40%
//   dist ≤ 20: 精细调节（20%）
//   step ≥ 0.5: 防止停滞
// 经实测，此参数组合在减速电机上表现最佳
// ***********************
void pid_update_setpoint(PIDController *c) {
    if (!c->ramp_enable || c->ramp_step == 0.0f) {
        c->setpoint = c->target;
        return;
    }

    float dist = fabsf(c->target - c->setpoint);
    float base_step = c->ramp_step;
    float step;

    if (dist > 30.0f) {
        step = base_step;                    // 全速逼近
    } else if (dist > 20.0f) {
        step = base_step * 0.4f;             // 减速
    } else {
        step = base_step * 0.2f;             // 精细调节
    }

    // 防止停滞
    if (step < 0.5f) step = 0.5f;

    if (c->setpoint < c->target) {
        c->setpoint = fminf(c->setpoint + step, c->target);
    } else if (c->setpoint > c->target) {
        c->setpoint = fmaxf(c->setpoint - step, c->target);
    }
}
//float pid_update(PIDController *c, float now) {

//	pid_update_setpoint(c); // // 更新 setpoint（带斜坡）

//    float error = c->setpoint - now;  // 使用 setpoint 计算误差

//    // 抗积分饱和：只有输出未饱和时才积分
//    if (c->output >= c->output_min && c->output <= c->output_max) {
//        c->integral += error * c->dt;
//    }

//    // 积分限幅
//    if (c->integral > c->integral_max) {
//        c->integral = c->integral_max;
//    } else if (c->integral < c->integral_min) {
//        c->integral = c->integral_min;
//    }

//    float derivative = (error - c->previous_error) / c->dt;
//    c->previous_error = error;

//    c->output = c->kp * error + 
//                c->ki * c->integral + 
//                c->kd * derivative;

//    // 输出限幅
//    if (c->output > c->output_max) {
//        c->output = c->output_max;
//    } else if (c->output < c->output_min) {
//        c->output = c->output_min;
//    }

//    return c->output;
//}
float pid_update(PIDController *c, float now) {
    pid_update_setpoint(c);           // 更新带斜坡的 setpoint
    float error = c->setpoint - now;

    // 1. 检测是否刚退出饱和
    int was_saturated = (c->output >= c->output_max || c->output <= c->output_min);
    float predicted_output = c->kp * error + c->ki * c->integral + c->kd * (error - c->previous_error) / c->dt;
    int is_now_saturated = (fabsf(error) > 1e-3) && 
                           (predicted_output >= c->output_max || predicted_output <= c->output_min);

    if (was_saturated && !is_now_saturated) {
        c->integral *= c->integral_damping_ratio;  // 衰减积分，如 0.3f
    }

    // 2. 抗饱和积分：仅在输出未饱和时累加
    if (c->output > c->output_min && c->output < c->output_max) {
        c->integral += error * c->dt;
    }

    // 3. 积分限幅
    if (c->integral > c->integral_max) {
        c->integral = c->integral_max;
    } else if (c->integral < c->integral_min) {
        c->integral = c->integral_min;
    }

    // 4. 微分项：对测量值微分（推荐）
    float derivative = 0.0f;
    if (c->dt > 0) {
        derivative = (now - c->previous_measurement) / c->dt;
    }
    c->previous_measurement = now;

    // 5. 计算输出（注意：微分项为负）
    c->output = c->kp * error + 
                c->ki * c->integral - 
                c->kd * derivative;

    // 6. 输出限幅
    if (c->output > c->output_max) {
        c->output = c->output_max;
    } else if (c->output < c->output_min) {
        c->output = c->output_min;
    }

    c->previous_error = error;
	c->error = c->target - now;
    return c->output;
}
